#include <ros/ros.h>
#include <std_msgs/Int8.h>
#include <std_msgs/UInt32.h>
#include <serial/serial.h>
#include <chrono>
#include <thread>
#include <collect_pkg/motor_control_node_plus.h>
#include <fcntl.h>
#include <stdlib.h>
#include <termios.h>
#include <unistd.h>
#include <iostream>

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "test_node");
    ros::NodeHandle nh;
    int publish_frequency = 1; // 1000 Hz (1秒发布1000次)
    int publish_duration = 4;    // 发布时长为10秒
    int speed = -4000;
    for (int i = 0; i < 2; i++)
    {
        double amplitude = 20.4;
        for (int j = 0; j < 2; j++)
        {
            double phase = 0;
            for (int k = 0; k < 1; k++)
            {
                ros::Rate loop_rate(publish_frequency);  // 控制发布频率
                ros::Time start_time = ros::Time::now(); // 记录开始时间
                while (ros::ok() && ros::Time::now().toSec() - start_time.toSec() <= publish_duration)
                { 
                    ROS_INFO_STREAM(speed);
                    ROS_INFO_STREAM(amplitude);
                    ROS_INFO_STREAM(phase);
                    loop_rate.sleep(); // 控制发布频率
                    
                }
                phase += 20;
            }
            amplitude += 60;
        }
        speed += 100;
    }
    return 0;
}
